//Mux control pins
int s0 = 5;
int s1 = 6;
int s2 = 7;
int s3 = 8;

//Mux in "SIG" pin
int SIG_pin = 0;
  
void setup() {
  initializeSonars();
  initializeMultiplex();
  Serial.begin(9600);
}

void initializeMultiplex() {
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT); 

  digitalWrite(s0, LOW);
  digitalWrite(s1, LOW);
  digitalWrite(s2, LOW);
  digitalWrite(s3, LOW);

}

void initializeSonars() {
  // 0v logic zero initially
  pinMode(2, OUTPUT);  
  digitalWrite(2, LOW);
  delay(300);  
  // pull pin "high"
  digitalWrite(2, HIGH);
  delayMicroseconds(25);
  pinMode(2, INPUT);  
  digitalWrite(2, HIGH);
}

int readSensor(int sensor) {
  int controlPin[] = {s0, s1, s2, s3};

  int muxChannel[16][4]={
    {0,0,0,0}, //channel 0
    {1,0,0,0}, //channel 1
    {0,1,0,0}, //channel 2
    {1,1,0,0}, //channel 3
    {0,0,1,0}, //channel 4
    {1,0,1,0}, //channel 5
    {0,1,1,0}, //channel 6
    {1,1,1,0}, //channel 7
    {0,0,0,1}, //channel 8
    {1,0,0,1}, //channel 9
    {0,1,0,1}, //channel 10
    {1,1,0,1}, //channel 11
    {0,0,1,1}, //channel 12
    {1,0,1,1}, //channel 13
    {0,1,1,1}, //channel 14
    {1,1,1,1}  //channel 15
  };

  //loop through the 4 sig
  for(int i = 0; i < 4; i ++){
    digitalWrite(controlPin[i], muxChannel[sensor][i]);
  }

  //read the value at the SIG pin
  int val = analogRead(SIG_pin);

  //return the value
  return val;
}

void loop() {

  int distance1 = readSensor(0);
  int distance2 = readSensor(1);
  int distance3 = readSensor(2);
  int distance4 = readSensor(3);
  int distance5 = readSensor(4);

  Serial.print(distance1);
  Serial.print("\t");
  Serial.print(distance2);
  Serial.print("\t");
  Serial.print(distance3);
  Serial.print("\t");
  Serial.print(distance4);
  Serial.print("\t");
  Serial.print(distance5);
  Serial.println("");
  
  delay(100);
}
